%==========================================================================
%       Vehicle Rolling Resistance and Aerodynamic Drag
%                           04/10/2015
%==========================================================================
clc
close all
clear all

% -----------------------Load Data----------------------------------------- 
run('LoadData');

% -----------------------Down Sampling------------------------------------- 
% original sample frequency 0.0035s / 3.5ms
% down sample rate 1/100 ( new sample frequency 0.35 sec)
down_sample_rate = 100; 
data_down_sampled = downsample(data,down_sample_rate); %down sample data
 
%-------------------------TUNNING FACTORS---------------------------------
 X = [4;1000];   %  CdL and Fr
 x = [4;1000];   %  CdL and Fr
 P = [3 0;0 100]; % covariance
 Minimal_Msurmnts = 10; % sample size need to be greater than this value to
                        % be send to the estimator

 table_size=20; % drag table # of rows
 
%--------------------------Inicializing -----------------------------------
 % Measurements
 y = 0;
 v_sqr = 0;
 z_counter = 0;
 grade = 0;
 is_extract = false ;
 previous_is_extract = false;

 % latteral motion 
 is_lateral = false ;
 lateral_states = [false,0,0]; % [is_lateral,buffer_counter,average];
 
 % constant speed
 is_constant_speed = false;
 constant_speed_states = [false,0]; % [is_const, buffer_counter]
 
 % convergence logic
 is_converged = false;
 converg_states = [X(1),X(2),1,500];  %[cdl, fr, dif_Cd_mean, dif_Cr_mean];
 
 % drag table
 drag_table=zeros(table_size,2);
 
 % Data record is for bebugging purpose. Remove in the real program
 Rec = [];  % general data
 Rec2 = []; % measurments
 Rec3 = []; % covergence

 % filtered result
 CdL_flt=x(1);
 Fr_flt=x(2);
 new_counter=0;
  
%% --------------------------Run_Simulator---------------------------------

disp('Simulator is Running,Please Wait ...')

for n=1:size(data_down_sampled,1) 
        % Stream the date to the simulator
        stream=data_down_sampled(n,:);         
        % Detact laterial motion
        [is_lateral,lateral_states] = LateralClassifier( stream(8),is_lateral,lateral_states );         
        % Detact constant speed motion
        [ is_constant_speed,constant_speed_states ] = ConstantSpeedClassifier( stream(2),stream(3),stream(7), is_constant_speed,constant_speed_states);
        % Determine whether to extract data 
        [ is_extract ] = IsExtracting( is_constant_speed,is_lateral );

       if is_extract== true          
                % take recursive average of the measurements
                [ v_sqr,y,grade,z_counter] = TakeMeasurements( stream,y,v_sqr,grade,z_counter,mass);
       else
            if (previous_is_extract == true )&&(z_counter> Minimal_Msurmnts) % has enough mesurement
                 
                %caculate weighting based on number of measuremenet or grade
                [ weight ] = AssignWeight( z_counter ,grade );
                % pass meaturement to table
                drag_table = UpdateTable( drag_table,v_sqr, y, weight,table_size );
                Rec2=[Rec2;[y v_sqr]];
       
                % caculate wegihted-RLS using the table, 
                % no need to caculate ever time
                new_counter = new_counter+z_counter;
                if new_counter>50
                %if Enough_Measurement(drag_table,table_size) && new_counter>50            
                    [ x,P ] = EvaluateTable( drag_table,x,P,table_size); 
                    [ is_converged,converg_states ] = CheckConvergence( x,converg_states );
                    CdL_flt = converg_states(1);
                    Fr_flt = converg_states(2);
                    new_counter = 0;
                
                end                
                % reset measurements
                z_counter=0;
                y=0;
                v_sqr=0;
                grade = 0;
            end
           
       end 
       previous_is_extract = is_extract; 

       % Record data
       Rec = [Rec;[stream,is_constant_speed,is_lateral,...
              lateral_states(2),converg_states(3),converg_states(4),...
              CdL_flt,Fr_flt]];
end
% ----------------------Plot Results---------------------------------------
run('RenderResults.m');
